clear all; close all;

samples = Samples('E5a',...
									'/home/ondrej/wnav/snapshots/snapshot_mem_05_e5a.dat');
E5aI_sc = SecondaryPRNCode('E5aI');
NumberOfSamples = floor(E5aI_sc.ChipPeriod/samples.SamplingPeriod);
NumberOfPeriods = 1;
E5aI_pc = PrimaryPRNCode('E5aI', NumberOfSamples, 1); 
E5aQ_pc = PrimaryPRNCode('E5aQ', NumberOfSamples, 1); 
samples.loadSamples(NumberOfSamples*NumberOfPeriods);

% Acquisition
acq = Acquisition('E5a', samples.FrequencyOffset);
SVID = 12;
fprintf(1, 'Acquiring E5a PRN %d ... \n', SVID);
acq.Acquire(samples, NumberOfPeriods, 1e-5, SVID, E5aI_pc, E5aQ_pc);
if (acq.DetectionStatus)
	fprintf(1, 'E5a PRN %d successfully acquired! \n', SVID);
	figure(SVID); clf;
	[m n] = size(acq.CCF);
	plot((1:m)*samples.SamplingPeriod, abs(acq.CCF),...
			 (1:m)*samples.SamplingPeriod, repmat(acq.Threshold, m, 1));
	xlabel('Time [s]');
	ylabel('CorrOut [-]');
	legend('CCF','Threshold');
end

% Tracking
if (acq.DetectionStatus)

	tracking = Tracking('E5a', SVID, samples, E5aI_pc, E5aQ_pc);
	tracking.setFrequencyShiftFilter(1, 5);
	tracking.setTimeShiftFilter(2, 50);
	tracking.initState(acq.TimeShift, acq.FrequencyShift);

	NumberOfCorrelations = 1000;
	TimeShiftMem = zeros(1, NumberOfCorrelations);
	FrequencyShiftMem = zeros(1, NumberOfCorrelations);
	[m n] = size(tracking.CorrelatorOut);
	CorrelatorOutMem = zeros(n, NumberOfCorrelations);

	for i=1:NumberOfCorrelations
		
		fprintf(1, 'Tracking number of correlation = %d \n', i);

		tracking.Track();
		TimeShiftMem(i) = tracking.TimeShift;
		FrequencyShiftMem(i) = tracking.FrequencyShift;
		CorrelatorOutMem(:,i) = tracking.CorrelatorOut.';
	end

	c = 3e8;
	t = (1:NumberOfCorrelations)*tracking.IntegrationTime;

	figure(1);clf;
	plot(t, TimeShiftMem*c);
	xlabel('Time [s]');
	ylabel('TimeShift [m]');
	
	figure(2);clf;
	plot(t, FrequencyShiftMem/samples.CarrierFrequency*c);
	xlabel('Time [s]');
	ylabel('FrequencyShift [m/s]');

	switch (n)
		case 3
			E = abs(CorrelatorOutMem(1,:));
			P = abs(CorrelatorOutMem(2,:));
			L = abs(CorrelatorOutMem(3,:));
		case 6
			E = abs(CorrelatorOutMem(1,:)) + abs(CorrelatorOutMem(4,:));
			P = abs(CorrelatorOutMem(2,:)) + abs(CorrelatorOutMem(5,:));
			L = abs(CorrelatorOutMem(3,:)) + abs(CorrelatorOutMem(6,:));
		otherwise
			error('Too many correlator outputs')
	end
	figure(3);clf;
	plot(t,	E, t, P, t, L);
	xlabel('Time [s]');
	ylabel('CorrelatorOut[-]');
	legend('Early', 'Prompt', 'Late');

else
	fprintf(1, 'SVID = %d not acquired.\n', SVID);
end

